#include <iostream>

#include <dir.h>
#include <cv.h>
#include <highgui.h>
#include <cvaux.h>

#include "all.h"
using namespace std;

#define WIDTH 100
#define HEIGHT 100

#define TIMES 50
//cmrRecognise  inputFolder faceClassifier
int main(int argc,char **argv)
{
    CvCapture * cmr=cvCreateCameraCapture(-1);
    cvNamedWindow("Camera",CV_WINDOW_AUTOSIZE);
    if(cmr == NULL)
    {
        cout<<"1";
        return 1;
    }

    CvHaarClassifierCascade* cascade = (CvHaarClassifierCascade*)
                                       cvLoad(argv[2], 0, 0, 0 );//Load XML file
    if(cascade == NULL)
    {
        cout<<"2";
        return 2;
    }

    //加载特征数据
    int length = strlen(argv[1]);
    char *IDPath = (char *)malloc(length+20);
    sprintf(IDPath,"%s\\IDMap.txt",argv[1]);
    char *xmlPath = (char *)malloc(length+20);
    sprintf(xmlPath,"%s\\eigenData.xml",argv[1]);
    CvFileStorage * fileStorage = cvOpenFileStorage(xmlPath, 0, CV_STORAGE_READ );
    if( fileStorage==NULL )
    {
        cout<<"3";
        return 3;
    }

    int imgPerMan = cvReadIntByName(fileStorage, 0, "imgPerMan", 0);
    int numPerson = cvReadIntByName(fileStorage, 0, "numPerson", 0);
    int numFace = cvReadIntByName(fileStorage, 0, "numFace", 0);
    int numEigen = cvReadIntByName(fileStorage, 0, "numEigen", 0);
    int realNum = cvReadIntByName(fileStorage, 0, "realNum", 0);
    CvMat *eigenValMat = (CvMat *)cvReadByName(fileStorage, 0, "eigenValMat", 0);
    CvMat *trainMat = (CvMat *)cvReadByName(fileStorage, 0, "trainMat", 0);
    IplImage *avgImg=(IplImage *)cvReadByName(fileStorage, 0, "avgImg", 0);

    //cout<<"begin load dstEigenArray"<<endl;
    //申请空间来存储加载的特征图像
    IplImage **dstEigenArray = (IplImage **)malloc(realNum*sizeof(IplImage *));

    for(int k=0; k<realNum; k++)
    {
        dstEigenArray[k] = cvCreateImage(cvSize(HEIGHT,WIDTH),IPL_DEPTH_32F,1);
        char *varname =(char *)malloc(50);
        sprintf( varname, "dstEigenArray%d", k );
        dstEigenArray[k] = (IplImage *)cvReadByName(fileStorage, 0, varname, 0);
        free(varname);
    }
    // release the file-storage interface
    cvReleaseFileStorage( &fileStorage );
    FILE *IDMapFile = fopen(IDPath,"r");
    int *IDdata = (int *)malloc(sizeof(int)*numPerson);

    int mahaFlag = 1;
    for(int i=0; i<realNum; i++)
    {
        if(fabs(eigenValMat->data.fl[i]) <=0.000001 )
        {
            mahaFlag = 0;
            break;
        }
    }

    //加载ID映射关系
    if(IDMapFile == NULL)
    {
        cout<<"4";
        return 4;
    }
    int tmp=0;
    for(int i=0; i<numPerson; i++)
    {
        fscanf(IDMapFile,"%d %d",&tmp,&(IDdata[i]));
    }

    //int i=0;
    CvMemStorage* storage = cvCreateMemStorage(0);
    CvSeq *faceSeq = NULL;
    IplImage *img=NULL;
    IplImage *gray=NULL;
    IplImage *face=NULL;
    int resultID[TIMES] = {0};
    int recTimes = 0;
    while(1)
    {
        img=cvQueryFrame(cmr);
        if(img == NULL)
            break;
        gray=cvCreateImage(cvGetSize(img),8,1);
        //cout<<"Height= "<<img->height<<"  Width= "<<img->width<<endl;
        cvCvtColor(img,gray,CV_RGB2GRAY);
        cvClearMemStorage( storage );
        faceSeq=(CvSeq*)cvHaarDetectObjects( gray, cascade, storage,1.2, 2, 1,cvSize(100, 100) );

        if(faceSeq == NULL || faceSeq->total <1)
        {
            continue;
        }

        //对一次识别进行五次


        CvRect * r = (CvRect*)cvGetSeqElem( faceSeq, faceSeq->total-1 );
        cvSetImageROI(gray,*r);
        face = cvCreateImage(cvSize(HEIGHT,WIDTH),8,1);
        cvResize(gray,face,CV_INTER_LINEAR);
        cvResetImageROI(gray);
        cvShowImage("face",face);

        draw_square(img,faceSeq);
        cvShowImage("Camera",img);
        char c=cvWaitKey(20);
        if(c==27)
            break;

        float *coefficient=(float *)malloc(realNum*sizeof(float));
        cvEigenDecomposite( face,
                            realNum,
                            dstEigenArray,
                            0, 0,
                            avgImg,
                            coefficient);

        double lstDist = DBL_MAX;
        int finalID = -1;
        if(mahaFlag == 0)
        {
            for(int iTrain=0; iTrain<numFace; iTrain++)
            {
                double distSq=0;
                //算出待识别图像距每一个训练图像的马氏距离distSq
                for(int j=0; j<realNum; j++)
                {
                    float d_j = coefficient[j] -trainMat->data.fl[iTrain*(realNum) + j];
                    distSq += d_j*d_j ; //欧氏距离
                    //distSq += d_j*d_j / eigenValMat->data.fl[j];  // Mahalanobis
                }
                //算出距此人的所有训练图像的平均距离
                //printf("%d-%d: dist is\t%f\n",iTrain/imgPerMan+1,iTrain%imgPerMan+1,distSq);
                //算出最小距离
                if(distSq < lstDist)
                {
                    lstDist = distSq;
                    finalID = iTrain;
                }
            }

            resultID[recTimes]=finalID/imgPerMan;
    //printf("%02d\n",resultID[recTimes]);
            recTimes ++;
        }
        else
        {
            for(int iTrain=0; iTrain<numFace; iTrain++)
            {
                double distSq=0;

                //算出待识别图像距每一个训练图像的马氏距离distSq
                for(int j=0; j<realNum; j++)
                {
                    float d_j = coefficient[j] -trainMat->data.fl[iTrain*(realNum) + j];
                    distSq += d_j*d_j / eigenValMat->data.fl[j];  // Mahalanobis
                }

                if(distSq < lstDist)
                {
                    lstDist = distSq;
                    finalID = iTrain;
                }
            }

            resultID[recTimes]=finalID/imgPerMan;
    //printf("%02d\n",resultID[recTimes]);
            recTimes ++;
        }

//printf("%02d\n",IDdata[finalID/imgPerMan]);


        if(recTimes>=TIMES)
            break;

    }

    int finalSeq =  getFreqSeq(resultID, recTimes);
    printf("%04d\n",IDdata[finalSeq]);
    //printf("%04d\n",IDdata[finalSeq/imgPerMan]);


    cvReleaseImage(&gray);
    cvReleaseImage(&face);
    cvReleaseCapture(&cmr);

    return 0;
}
